#include "swegl/math.h"

namespace swegl
{

const Matrix4x4 Matrix4x4::Zero(Vec4(0, 0, 0, 0), Vec4(0, 0, 0, 0), Vec4(0, 0, 0, 0), Vec4(0, 0, 0, 0));
const Matrix4x4 Matrix4x4::Identity(Vec4(1, 0, 0, 0), Vec4(0, 1, 0, 0), Vec4(0, 0, 1, 0), Vec4(0, 0, 0, 1));

Matrix4x4 Matrix4x4::operator * (const Matrix4x4 &rhs) const
{
	return Matrix4x4(
	           m[0].x * rhs[0].x + m[0].y * rhs[1].x + m[0].z * rhs[2].x + m[0].w * rhs[3].x,
			   m[0].x * rhs[0].y + m[0].y * rhs[1].y + m[0].z * rhs[2].y + m[0].w * rhs[3].y,
	           m[0].x * rhs[0].z + m[0].y * rhs[1].z + m[0].z * rhs[2].z + m[0].w * rhs[3].z,
			   m[0].x * rhs[0].w + m[0].y * rhs[1].w + m[0].z * rhs[2].w + m[0].w * rhs[3].w,
			   
	           m[1].x * rhs[0].x + m[1].y * rhs[1].x + m[1].z * rhs[2].x + m[1].w * rhs[3].x,
			   m[1].x * rhs[0].y + m[1].y * rhs[1].y + m[1].z * rhs[2].y + m[1].w * rhs[3].y,
	           m[1].x * rhs[0].z + m[1].y * rhs[1].z + m[1].z * rhs[2].z + m[1].w * rhs[3].z,
			   m[1].x * rhs[0].w + m[1].y * rhs[1].w + m[1].z * rhs[2].w + m[1].w * rhs[3].w,

	           m[2].x * rhs[0].x + m[2].y * rhs[1].x + m[2].z * rhs[2].x + m[2].w * rhs[3].x,
			   m[2].x * rhs[0].y + m[2].y * rhs[1].y + m[2].z * rhs[2].y + m[2].w * rhs[3].y,
	           m[2].x * rhs[0].z + m[2].y * rhs[1].z + m[2].z * rhs[2].z + m[2].w * rhs[3].z,
			   m[2].x * rhs[0].w + m[2].y * rhs[1].w + m[2].z * rhs[2].w + m[2].w * rhs[3].w,

	           m[3].x * rhs[0].x + m[3].y * rhs[1].x + m[3].z * rhs[2].x + m[3].w * rhs[3].x,
			   m[3].x * rhs[0].y + m[3].y * rhs[1].y + m[3].z * rhs[2].y + m[3].w * rhs[3].y,
	           m[3].x * rhs[0].z + m[3].y * rhs[1].z + m[3].z * rhs[2].z + m[3].w * rhs[3].z,
			   m[3].x * rhs[0].w + m[3].y * rhs[1].w + m[3].z * rhs[2].w + m[3].w * rhs[3].w
			   );
}

void Matrix4x4::lookAt(const Vec3 &from, const Vec3 &at, const Vec3 &up)
{
	Vec3 zAxis = at - from;
	float l = zAxis.length();
	const float Epsilon = 0.001f;
	if (l < Epsilon)
	{

	}
	else
	{
		zAxis *= (1 / l);
	}
	Vec3 xAxis = up.cross(zAxis);
	xAxis.normalize();
	Vec3 yAxis = zAxis.cross(xAxis);
	yAxis.normalize();

	m[0].x = xAxis.x;
	m[0].y = yAxis.x;
	m[0].z = zAxis.x;
	m[0].w = 0;
	m[1].x = xAxis.y;
	m[1].y = yAxis.y;
	m[1].z = zAxis.y;
	m[1].w = 0;
	m[2].x = xAxis.z;
	m[2].y = yAxis.z;
	m[2].z = zAxis.z;
	m[2].w = 0;

	m[3].x = -from * xAxis;
	m[3].y = -from * yAxis;
	m[3].z = -from * zAxis;
	m[3].w = 1;
}

void Matrix4x4::objectLookAt(const Vec3 &from, const Vec3 &at, const Vec3 &up)
{
	Vec3 zAxis = at - from;
	float l = zAxis.length();
	const float Epsilon = 0.001f;
	if (l < Epsilon)
	{

	}
	else
	{
		zAxis *= (1 / l);
	}
	Vec3 xAxis = up.cross(zAxis);
	xAxis.normalize();
	Vec3 yAxis = zAxis.cross(xAxis);
	yAxis.normalize();

	m[0].x = xAxis.x;
	m[0].y = xAxis.y;
	m[0].z = xAxis.z;
	m[0].w = 0;
	m[1].x = yAxis.x;
	m[1].y = yAxis.y;
	m[1].z = yAxis.z;
	m[1].w = 0;
	m[2].x = zAxis.x;
	m[2].y = zAxis.y;
	m[2].z = zAxis.z;
	m[2].w = 0;

	m[3].x = from.x;
	m[3].y = from.y;
	m[3].z = from.z;
	m[3].w = 1;
}

void Matrix4x4::setPerspective(float fovY, float aspectRatio, float zNear, float zFar)
{
	sweglAssert(fovY > 0);
	sweglAssert(aspectRatio > 0);
	
	float tanThetaY = Tan(fovY * 0.5f);
	float tanThetaX = tanThetaY * aspectRatio;
	float half_w = tanThetaX * zNear;
	float half_h = tanThetaY * zNear;

	float right = half_w;
	float left = -half_w;
	float top = half_h;
	float bottom = -half_h;

	// [ w   0   0   0 ]
	// [ 0   h   0   0 ]
	// [ 0   0   q   1 ]
	// [ 0   0   qn  0 ]
	*this = Zero;
	m[0].x = 2 * zNear / (right - left);
	m[1].y = 2 * zNear / (top - bottom);
	m[2].x = (left + right) / (left - right);
	m[2].y = (bottom + top) / (bottom - top);
	m[2].z = (zFar + zNear) / (zFar - zNear);
	m[2].w = 1;
	m[3].z = 2 * zNear * zFar / (zNear - zFar);
}

void Matrix4x4::setOrtho(float left, float right, float bottom, float top, float zNear, float zFar)
{
	*this = Matrix4x4(
		2 / (right - left)             , 0                              , 0                              , 0,
		0                              , 2 / (top - bottom)             , 0                              , 0,
		0                              , 0                              , 2 / (zFar - zNear)             , 0,
		(left + right) / (left - right), (bottom + top) / (bottom - top), (zNear + zFar) / (zNear - zFar), 1
		);
}

void Matrix4x4::setRotation(const Matrix3x3 &rotation)
{
	m[0].x = rotation[0][0];
	m[0].y = rotation[0][1];
	m[0].z = rotation[0][2];
	m[1].x = rotation[1][0];
	m[1].y = rotation[1][1];
	m[1].z = rotation[1][2];
	m[2].x = rotation[2][0];
	m[2].y = rotation[2][1];
	m[2].z = rotation[2][2];
}

void Matrix4x4::setTranslation(const Vec3 &translation)
{
	m[3].x = translation.x;
	m[3].y = translation.y;
	m[3].z = translation.z;
}

Matrix3x3 Matrix4x4::rotation() const
{
	return Matrix3x3(
		m[0].x, m[0].y, m[0].z,
		m[1].x, m[1].y, m[1].z,
		m[2].x, m[2].y, m[2].z);
}

Vec3 Matrix4x4::translation() const
{
	return Vec3(m[3].x, m[3].y, m[3].z);
}

void Matrix4x4::affineInvert()
{
	// negate inverse rotated translation part
	float tx = m[0].toVec3() * m[3].toVec3();
	float ty = m[1].toVec3() * m[3].toVec3();
	float tz = m[2].toVec3() * m[3].toVec3();
	m[3].x = - tx;
	m[3].y = - ty;
	m[3].z = - tz;

	// transpose rotation part
	Swap(m[0].y, m[1].x);
	Swap(m[0].z, m[2].x);
	Swap(m[1].z, m[2].y);
}

void Matrix4x4::invert()
{
	float m00 = m[0].x, m01 = m[1].x, m02 = m[2].x, m03 = m[3].x;
	float m10 = m[0].y, m11 = m[1].y, m12 = m[2].y, m13 = m[3].y;
	float m20 = m[0].z, m21 = m[1].z, m22 = m[2].z, m23 = m[3].z;
	float m30 = m[0].w, m31 = m[1].w, m32 = m[2].w, m33 = m[3].w;

	float v0 = m20 * m31 - m21 * m30;
	float v1 = m20 * m32 - m22 * m30;
	float v2 = m20 * m33 - m23 * m30;
	float v3 = m21 * m32 - m22 * m31;
	float v4 = m21 * m33 - m23 * m31;
	float v5 = m22 * m33 - m23 * m32;

	float t00 = + (v5 * m11 - v4 * m12 + v3 * m13);
	float t10 = - (v5 * m10 - v2 * m12 + v1 * m13);
	float t20 = + (v4 * m10 - v2 * m11 + v0 * m13);
	float t30 = - (v3 * m10 - v1 * m11 + v0 * m12);

	float invDet = 1 / (t00 * m00 + t10 * m01 + t20 * m02 + t30 * m03);

	float d00 = t00 * invDet;
	float d10 = t10 * invDet;
	float d20 = t20 * invDet;
	float d30 = t30 * invDet;

	float d01 = - (v5 * m01 - v4 * m02 + v3 * m03) * invDet;
	float d11 = + (v5 * m00 - v2 * m02 + v1 * m03) * invDet;
	float d21 = - (v4 * m00 - v2 * m01 + v0 * m03) * invDet;
	float d31 = + (v3 * m00 - v1 * m01 + v0 * m02) * invDet;

	v0 = m10 * m31 - m11 * m30;
	v1 = m10 * m32 - m12 * m30;
	v2 = m10 * m33 - m13 * m30;
	v3 = m11 * m32 - m12 * m31;
	v4 = m11 * m33 - m13 * m31;
	v5 = m12 * m33 - m13 * m32;

	float d02 = + (v5 * m01 - v4 * m02 + v3 * m03) * invDet;
	float d12 = - (v5 * m00 - v2 * m02 + v1 * m03) * invDet;
	float d22 = + (v4 * m00 - v2 * m01 + v0 * m03) * invDet;
	float d32 = - (v3 * m00 - v1 * m01 + v0 * m02) * invDet;

	v0 = m21 * m10 - m20 * m11;
	v1 = m22 * m10 - m20 * m12;
	v2 = m23 * m10 - m20 * m13;
	v3 = m22 * m11 - m21 * m12;
	v4 = m23 * m11 - m21 * m13;
	v5 = m23 * m12 - m22 * m13;

	float d03 = - (v5 * m01 - v4 * m02 + v3 * m03) * invDet;
	float d13 = + (v5 * m00 - v2 * m02 + v1 * m03) * invDet;
	float d23 = - (v4 * m00 - v2 * m01 + v0 * m03) * invDet;
	float d33 = + (v3 * m00 - v1 * m01 + v0 * m02) * invDet;

	m[0].set(d00, d10, d20, d30);
	m[1].set(d01, d11, d21, d31);
	m[2].set(d02, d12, d22, d32);
	m[3].set(d03, d13, d23, d33);
}

Matrix4x4 Matrix4x4::Invert(const Matrix4x4 &m)
{
	Matrix4x4 inv(m);
	inv.invert();
	return inv;
}

Vec3 Matrix4x4::operator * (const Vec3 &v) const
{
	return m[0].toVec3() * v.x + m[1].toVec3() * v.y + m[2].toVec3() * v.z + m[3].toVec3();
}

Vec4 Matrix4x4::transform(const Vec4 &v) const
{
	return m[0] * v.x + m[1] * v.y + m[2] * v.z + m[3] * v.w;
}

}

